! This represents  n_modulation + N_spin = integer resonance
!
! This is a program which uses a FODO cell conconted by Zhe Duan represented by a flat file
!  
! The entire acceleration is faked more properly. No Lucio trick.
!

program ptc_geometry
use madx_ptc_module
use pointer_lattice
use c_TPSA
implicit none

type(probe) xs0,XST
type(probe_8) xs
type(layout), pointer :: fodo
real(dp) closed(6),theta0,N0(3),X0(6), closed_orbit1(6),p0,pf
type(real_8) ray8(6),theta
INTEGER MF,I,k,mft,pos
TYPE(FIBRE),POINTER:: P
type(internal_state) state
type(damapspin) id_s,one_turn_S,a_S,A_f,A_spin,A_l,A_nl,NORMALSPIN
type(normal_spin) nf_S
TYPE(res_spinor_8) N_AXIS_res
real(dp) dr,ag,ag0,gamma0
real(dp) circ,prec 
integer doit

complex(dp) xc(6) , bet, dtune
TYPE(work) E_0
REAL(DP) rate
type(c_ray) cray,xcn
!-----------------------------------

type(c_damap) c_map,c_spin0
type(c_normal_form) c_n
TYPE(c_spinor) C_N0
integer nturn
real(dp) epsnx, epsny, epsx,  epsy, betx, bety, alfx, alfy, &
      ramprate, Ggai, ggamma0, Ggaf, ek,&
    resstrength, fpol, sigma(2),  ggamma, ga,xsig(lnv)

real(dp):: p0c, p0c0, csinvariant
open(1, file="fr.in", status="old")
  read(1,*)epsnx, epsny
  read(1,*)ramprate, Ggai,ggamma0, Ggaf, ek
  close(1)

  resstrength=pi*(ek**2)/2.0d0/ramprate
  fpol=2.0d0*exp(-resstrength)-1
  write(*,*) "The spin resonance strength is", resstrength
  write(*,*)"The expected final polarization of resonance crossing is", fpol
  nturn=int((Ggaf-Ggai)/(twopi*ramprate)) 
  write(*,*)"The total tracking turns is", nturn

spin_tune_def=-1
call ptc_ini_no_append

CALL  READ_AND_APPEND_VIRGIN_general(M_U,"fodoflat.txt")
m_U%start%name="fodo "
write(6,*) "Making an node layout"
CALL MAKE_NODE_LAYOUT(m_U%start)

fodo=>m_U%start
p=>fodo%start


!!!! circ is the circumference of the ring !!!! 
call get_length(fodo,circ)


p=>fodo%start
ag0=p%ag
gamma0=ggamma0/ag0
  write(*,*)"Ggamma is"
  write(*,*)ag0
  write(*,*)gamma0
  write(*,*)ggamma0


closed=0.d0
state=default0+nocavity0  

    CALL FIND_ORBIT(fodo,CLOSED,1,STATE,c_1d_5)
 
call kanalnummer(mf,"spin_result.txt")

state=default0+only_4d0+SPIN0
XS0=CLOSED
CALL FIND_ORBIT_probe_spin(fodo,XS0,STATE,c_1d_5,FIBRE1=1,theta0=theta0)

WRITE(6,*) XS0%X
WRITE(6,*) XS0%S(1)%X
WRITE(6,*) THETA0/twopi
N0=XS0%S(1)%X
write(6,*) " n0 "
write(6,*) n0


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
CALL INIT_all(STATE,3,0)
call alloc(c_map)
call alloc(c_n)
call alloc(c_spin0)
CALL ALLOC(C_N0)

call alloc(xs)
call alloc(ray8)
call alloc(theta)
call alloc(N_AXIS_res)
call alloc(id_S,one_turn_S,A_S,A_f,A_spin,A_l,A_nl,NORMALSPIN)
call alloc(nf_S)

!!!! Polymorphic probe is created in the usual manner   
   ID_S=1
   
   XS=XS0+ID_S     !
   
!!!! get spin polymorphic probe after one turn   
CALL TRACK_PROBE(fodo,XS,STATE,FIBRE1=1)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 


!!!! Copy probe_8 into a damapspin
ID_S=XS
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 



!!! Map normalised
NF_S=ID_S
write(6,*) "nx,ny,nu_modulation,nu_spin"
write(6,'(4(1x,E15.7))') NF_S%n%tune(1:c_%nd),NF_S%nu


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!! compute the twiss parameters at the current position
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  
  betx=(NF_S%N%a_t%v(1).sub.'1')**2+(NF_S%N%a_t%v(1).sub.'01')**2
  bety=(NF_S%N%a_t%v(3).sub.'001')**2+(NF_S%N%a_t%v(3).sub.'0001')**2
  alfx=-(NF_S%N%a_t%v(1).sub.'1')*(NF_S%N%a_t%v(2).sub.'1')-&
       (NF_S%N%a_t%v(1).sub.'01')*(NF_S%N%a_t%v(2).sub.'01')
  alfy=-(NF_S%N%a_t%v(3).sub.'001')*(NF_S%N%a_t%v(4).sub.'001')-&
       (NF_S%N%a_t%v(3).sub.'0001')*(NF_S%N%a_t%v(4).sub.'0001')

  write(*,*)"fractional tunes "
  write(*,*) NF_S%N%tune(1:2)
  write(*,*) "betx, bety, alfx, alfy are"
  write(*,*) betx, bety, alfx, alfy

!!!!!!!!! Horizontal & vertical emittances

!  epsx=epsnx/sqrt(gamma0**2-1.0d0)/6.0d0 
!  epsy=epsny/sqrt(gamma0**2-1.0d0)/6.0d0 
  ga=Ggai/ag0
  epsx=epsnx/sqrt(ga**2-1.0d0)/6.0d0 
  epsy=epsny/sqrt(ga**2-1.0d0)/6.0d0 
  call find_energy(e_0, gamma=gamma0)
  p0c0=e_0%p0c
  sigma(1)=sqrt(betx*epsx)
  sigma(2)=sqrt(bety*epsy)
  write(*,*)"The horizontal emittance is ",epsx
  write(*,*)"The vertical emittance is ", epsy

  write(*,*)"The horizontal beam size is ",sigma(1)
  write(*,*)"The vertical beam size is ",sigma(2)


write(mf,*) " "
write(mf,*) " Here I set the initial ray of the tracking"
write(mf,*) " "

close(mf)

doit=0
 
!!!!  Here checking with Tracking

xsig(1:6)=closed
xsig(1)=sqrt(epsx)
xsig(3)=sqrt(epsy)

xsig=NF_S%N%a_t*xsig

x0=0.0d0
x0(3)=sigma(2)

write(6,*) xsig(1),xsig(2)
write(6,*) xsig(3),xsig(4),x0(3)



XS0%X(3)=XS0%X(3)+X0(3)
XS0%s(1)%x(1)=0.d0
XS0%s(1)%x(2)=1.d0
XS0%s(1)%x(3)=0.d0


!333 format(i5, 1x, e16.9, 6(1x, e12.4) )
444 format(i5, 1x, e16.9, 7(1x, e12.4) )
call kanalnummer(mft,"spin_energy_ramp.dat")
ggamma=Ggai

do i=1,nturn   !+5000
  
  gamma0=ggamma/ag0
  call find_energy(E_0, gamma=gamma0)
  e_0%rescale=.false.
  if(i.eq.1) then
    call print_work(e_0, 9)
   p0=E_0%p0c
   endif 
 fodo=e_0    ! layout_work
  if(i.GT.1) then
     xs0%x(2)=p0c/e_0%p0c*xs0%x(2) !! assuming there is no solenoid
     !!component here
     xs0%x(4)=p0c/e_0%p0c*xs0%x(4) 
    
  endif
  p0c=e_0%p0c

  p=>fodo%start
  ga=P%BETA0/P%GAMMA0I*( 1.0_dp/P%BETA0 + xs0%X(5) )


 csinvariant=2*(xs0%x(3)**2+(alfy*xs0%x(3)+bety*xs0%x(4))**2)/bety*p0c/p0c0
 closed_orbit1=0.d0 ! initial guess for closed orbit
 xst=closed_orbit1

 call FIND_ORBIT_probe_spin(fodo,xst,state,1.d-5,fibre1=pos,theta0=theta0)
 closed_orbit1=xst%x

! write(mft,333)i,ggamma,theta0/2.0d0/pi,  xs0%x(1:4),  xs0%s(1)%x(2),csinvariant
 write(mft,444)i,ggamma,theta0/2.0d0/pi,  xs0%x(1:4), xs0%s(1)%x(2),csinvariant
 CALL TRACK_PROBE(fodo,XS0,STATE,FIBRE1=1)


if(ggamma>42.7660d0.and.doit<1) then

call kanalnummer(mf,"spin_result_orbit.txt")
call FIND_ORBIT_probe_spin(fodo,xst,state,1.d-5,fibre1=pos,theta0=theta0)
 
 !!!! Polymorphic probe is created in the usual manner   
   ID_S=1
   
   XS=xst+ID_S     !
   
!!!! get spin polymorphic probe after one turn   
CALL TRACK_PROBE(fodo,XS,STATE,FIBRE1=1)
!!!! Copy probe_8 into a complex damap because we use the complex package for normal forms
c_map=XS
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 
 
!!!! one resoanance is left in the map 
c_n%nres=1
c_n%m(2,1)=1   !  the  
c_n%ms(1)=1    ! spin tune
!    so the resonance is   n_y + N_spin = integer
call c_normal(c_map,c_n,dospin=my_true)
prec=1.d-6
! printing all the tunes
write(6,*) c_n%tune(1:c_%nd)

write(6,*) " spin ",c_n%spin_tune,ggamma
!  Here I put A_t in phasor's basis!!!    
c_n%A_t=c_n%A_t*from_phasor()   

! m = As a n a^-1 As^-1
c_map=c_n%A_t**(-1)*c_n%As**(-1)*c_map*c_n%As*c_n%A_t
 
write(mf,*) " Normalised map "
call  print(c_map,mf,DEPS=prec)

!!! C_spin0 is a complex map
c_spin0=1   !!! identity

!!!! Only contains linear part of spin, 0-zeroth order matrix
C_SPIN0%S=c_map%S.SUB.0

write(mf,*) " Linear part of the spin only  "
call  print(C_SPIN0,mf,DEPS=prec)

! Here the map is factorised as map :   exp(theta0 L_y) o ( r, exp(a _y + resonance))
! C_SPIN0 =( r, exp(a _y + resonance))

C_SPIN0=C_SPIN0**(-1)*C_MAP

write(mf,*) " "
write(mf,*) " Here the map is factorised as map :   "
write(mf,*) "  exp(theta0 L_y) o ( r, exp(a _y + resonance)) "
write(mf,*) "   ( r, exp(a _y + resonance)) is printed"
write(mf,*) "   "

call  print(C_SPIN0,mf,DEPS=prec)


! a _y + resonance = C_n0
write(mf,*) " "
write(mf,*) "   a _y + resonance = C_n0  "
write(mf,*) " "
          call c_find_n0_da(c_spin0%s,C_N0)
          call c_n0_to_nr(C_N0,C_N0)
write(mf,*) " C_n0 is  "

Write(mf,*) " C_n0%v(1)  < =   C_n0%v(1)-i_*C_n0%v(3) ! coefficient of  1/2(L_x + i L_z) "
Write(mf,*) " C_n0%v(2)  < =   C_n0%v(2)"
Write(mf,*) " C_n0%v(3)  < =   C_n0%v(1)+i_*C_n0%v(3) ! coefficient of  1/2(L_x - i L_z) "
write(mf,*) " "
CALL PRINT(C_N0,mf,PREC)

!!! Here starts some evaluation of the various operators

write(mf,*) " "
write(mf,*) " Here I set the initial ray of the tracking"
write(mf,*) " "
!!! These are in initial coordinates

Xc=0.D0
xc=XS0%x-XSt%x
XcN%x=0.D0

write(6,*)
cray=xc
write(mf,'(6(1x,g12.5))') xc(1:4)

xcn=(c_n%A_T**(-1)).o.cray

   pf=E_0%p0c
 
write(6,*) "cheated value of emittance ", real(xcn%x(3)*xcn%x(4))   ! (y + i py) * (y - i py)
write(6,*) " from  original value  ", epsy*p0/pf

write(mf,*) " "
write(mf,*) " Evaluate the resonance and tune shift"
write(mf,*) " "

bet=c_n0%v(1).o.xcn

dtune=(c_n0%v(2).o.xcn)/twopi



write(mf,*) " Numerical value of beta ",bet

write(mf,*) " Numerical value of beta*beta*",abs(bet)**2

write(mf,*) " Numerical spin tune shift ",dtune 


dr=ramprate*twopi

rate=abs(bet)**2/dr/4.d0

write(6,*)"formula with complex DA and cheated value", 2*exp(-rate)-1
write(mf,*)"formula with complex DA and cheated value", 2*exp(-rate)-1

xcn%x=0.d0
xcn%x(3)=sqrt(epsy*p0/pf)
xcn%x(4)=sqrt(epsy*p0/pf)

bet=c_n0%v(1).o.xcn

dtune=(c_n0%v(2).o.xcn)/twopi

rate=abs(bet)**2/dr/4.d0
write(6,*) "formula with complex DA  using initial emittance ", 2*exp(-rate)-1
write(mf,*)"formula with complex DA  using initial emittance ", 2*exp(-rate)-1



doit=doit+1
close(mf)
endif

 ggamma=ggamma+ramprate*twopi
enddo

close(mft)








    call KILL(ray8)
    call KILL(id_S,one_turn_S,A_S,A_f,A_spin,A_l,A_nl,NORMALSPIN)
    call KILL(nf_S)
    call KILL(xs)
    call KILL(theta)
    call KILL(N_AXIS_res)




 call ptc_end


end program ptc_geometry




!A_S=NF_S%A_T

!!!! factor the "Courant-Snyder" transformation
!call factor(A_S,A_f,A_spin,A_l,A_nl)  ! -->>>   (A_S%m,A_S%s) = (a_f%m, I ) o (I ,A_spin%s) o (a_l%m,I) o (a_nl%m,I)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!



